/*
 * @author Ted Meyer
 */

package edu.wpi.first.wpilibj.robot;

import edu.wpi.first.wpilibj.*;


public class DriveMotor extends Motor{
	Joystick theJoy;
        boolean shooting = false;
        PrintMotor pm;
	
	public DriveMotor(Jaguar jag, Joystick joy){
		super(jag);
		theJoy = joy;
	}
	
	public DriveMotor(Jaguar[] jags, Joystick joy, PrintMotor pm){
            super(jags);
            this.pm = pm;
            theJoy = joy;
	}
	
	public void run(){
            try{
		while((theJags!=null || theJag!=null) && theJoy!=null){
                    double magnitude = theJoy.getMagnitude();
                    double radians = theJoy.getDirectionRadians()+Math.PI/4;

                    int a = 0; int b = 1;
                    if (theJags[0]!=null && theJags[1]!=null && !shooting){
                        double aa = -Math.sin(radians)*Math.abs(magnitude)/.707;
                        double bb = Math.cos(radians)*Math.abs(magnitude)/.707;
                        
                        theJags[a].set(aa);
                        theJags[b].set(bb);
                        pm.setText("                                   ", 2);
                    }
                    else{
                        theJags[a].set(0);
                        theJags[b].set(0);
                        pm.setText("fustercluck pew pew pew pew", 2);
                    }
		}
            }
            catch(Exception ee){
                pm.setText("FAILZ        ", 2);
            }
	}
}
